#include "Mahony.h"
#include <math.h>
#include <arm_math.h>
#include "bmi088.h"
#include "can.h"
#include "interface.h"
#include "tim.h"

Mahony_TypeDef Mahony_Data={0};
int cal_time=0;
	
float invSqrt(float x)
{
    float halfx = 0.5f * x;
    float y = x;
    long i = *(long*)&y;
    i = 0x5f3759df - (i>>1);
    y = *(float*)&i;
    y = y * (1.5f - (halfx * y * y));
    return y;
}

void Mahony_init(Mahony_TypeDef* m,int n,float time,float KP1, float KP2,float KI1,float KI2,float KD)
{
	m->n = n;
	m->Mahony_Filiter[0].n=n;
	m->Mahony_Filiter[1].n=n;
	m->Mahony_Filiter[2].n=n;
    m->delta_time = time;
    m->KP1 = KP1;
	m->KP2 = KP2;
    m->KI1 = KI1;
	m->KI2 = KI2;
	m->KD=KD;
    m->q0 = 1.0;  // 初始化四元数为单位四元数
    m->q1 = 0;
    m->q2 = 0;
    m->q3 = 0;
    m->exInt = 0;
    m->eyInt = 0;
    m->ezInt = 0;
}

void Mahony_adapt(Mahony_TypeDef* m,float gz)
{
	if(gz>=0.05||gz<=-0.05)
	{
		m->KP2=5;
	}
	else
	{
		m->KP2=1;
	}
}

void Mahony_Error_Cal(Mahony_Filiter_TypeDef* m,float x)
{
	if(m->cnt==m->n)
	{
		m->average=m->sum/m->n;
		m->flag=1;
		return;
	}
	m->sum+=x;
	m->cnt++;
}

float Mahony_bessel_filter(Mahony_Last_TypeDef* m, float now, float filter)
{
    // 计算滤波后的值
    float temp = 1 * (1 - filter) * (1 - filter) * (1 - filter) * (1 - filter) * (1 - filter) * now + 
                 5 * (1 - filter) * (1 - filter) * (1 - filter) * (1 - filter) * filter * m->last_0 +
                 10 * (1 - filter) * (1 - filter) * (1 - filter) * filter * filter * m->last_1 +
                 10 * (1 - filter) * (1 - filter) * filter * filter * filter * m->last_2 +
                 5 * (1 - filter) * filter * filter * filter * filter * m->last_3 +
                 1 * filter * filter * filter * filter * filter * m->last_4;

    // 更新历史值
    m->last_4 = m->last_3;
    m->last_3 = m->last_2;
    m->last_2 = m->last_1;
    m->last_1 = m->last_0;
    m->last_0 = now;
	return temp;
}

float IMU_gaussian_filter(Mahony_Last_TypeDef* m, float now)
{
    // 7点高斯滤波系数计算（sigma=1.0时的标准系数）
    // 如果需要动态sigma，可以实时计算系数
    static const float gaussian_coeffs[7] = {
        0.006,   // 第-3个点
        0.061,   // 第-2个点  
        0.242,   // 第-1个点
        0.382,   // 中心点 (第0个点)
        0.242,   // 第+1个点
        0.061,   // 第+2个点
        0.006    // 第+3个点
    };
    
    // 计算滤波后的值
    float temp = gaussian_coeffs[0] * m->last_6 +   // 最旧的数据
                 gaussian_coeffs[1] * m->last_5 +
                 gaussian_coeffs[2] * m->last_4 +
                 gaussian_coeffs[3] * m->last_3 +   // 中心附近的数据
                 gaussian_coeffs[4] * m->last_2 +
                 gaussian_coeffs[5] * m->last_1 +
                 gaussian_coeffs[6] * now;          // 最新的数据
    
    // 更新历史值（需要扩展数据结构以存储7个历史值）
    m->last_6 = m->last_5;
    m->last_5 = m->last_4;
    m->last_4 = m->last_3;
    m->last_3 = m->last_2;
    m->last_2 = m->last_1;
    m->last_1 = m->last_0;
    m->last_0 = now;
    
    return temp;
}

void Mahony_update(Mahony_TypeDef* m,float gx, float gy, float gz,float ax, float ay, float az)
{
	m->gx=Mahony_bessel_filter(&m->Last[0],gx,0.12);
	m->gy=Mahony_bessel_filter(&m->Last[1],gy,0.12);
	m->gz=Mahony_bessel_filter(&m->Last[2],gz,0.25);
	m->ax=Mahony_bessel_filter(&m->Last[4],ax,0.12);
	m->ay=Mahony_bessel_filter(&m->Last[5],ay,0.12);
	m->az=Mahony_bessel_filter(&m->Last[6],az,0.12);
//	m->gx=IMU_gaussian_filter(&m->Last[0],gx);
//	m->gy=IMU_gaussian_filter(&m->Last[1],gy);
//	m->gz=IMU_gaussian_filter(&m->Last[2],gz);
//	m->ax=IMU_gaussian_filter(&m->Last[4],ax);
//	m->ay=IMU_gaussian_filter(&m->Last[5],ay);
//	m->az=IMU_gaussian_filter(&m->Last[6],az);
}

void Mahony_cal(Mahony_TypeDef* m)
{
    float gx = m->gx, gy = m->gy, gz = m->gz;
    float ax_i = m->ax, ay_i = m->ay, az_i = m->az;
    float exInt = m->exInt, eyInt = m->eyInt, ezInt = m->ezInt;
    float q0 = m->q0, q1 = m->q1, q2 = m->q2, q3 = m->q3;
	
	if (q0 == 0.0f && q1 == 0.0f && q2 == 0.0f && q3 == 0.0f) 
    {
        q0 = 1.0f;  // 重新初始化为单位四元数
    }
	
    float norm = sqrt(ax_i * ax_i + ay_i * ay_i + az_i * az_i);
	if (norm == 0.0f) 
	{
		ax_i = 0.0f;
		ay_i = 0.0f;
		az_i = 1.0f; 
	} 
	else 
	{
		ax_i = ax_i / norm;
		ay_i = ay_i / norm;
		az_i = az_i / norm;
	}

    float vx = 2 * (q1 * q3 - q0 * q2);
    float vy = 2 * (q0 * q1 + q2 * q3);
    float vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

    float ex = (ay_i * vz - az_i * vy);
    float ey = (az_i * vx - ax_i * vz);
    // 增强 yaw 修正项：加入小比例的 roll/pitch误差耦合
	float ez = (ax_i * vy - ay_i * vx) + 0.05f * (ex + ey);
	
    exInt += ex * m->KI1;
    eyInt += ey * m->KI1;
    ezInt += ez * m->KI2;
	
	if(exInt>=0.05)
	{
		exInt =0.05;
	}
	if(exInt<=-0.05)
	{
		exInt =-0.05;
	}

	if(eyInt>=0.05)
	{
		exInt =0.05;
	}
	if(eyInt<=-0.05)
	{
		eyInt =-0.05;
	}
	
	if(ezInt>=0.05)
	{
		ezInt =0.05;
	}
	if(ezInt<=-0.05)
	{
		ezInt =-0.05;
	}
	
    gx = gx + m->KP1 * ex + exInt;
    gy = gy + m->KP1 * ey + eyInt;
    gz = gz + m->KP2 * ez + ezInt;
	
	if(fabs(norm-1.0f) < 0.07f)  // 基本静止
	{
		 gz = 0.0f;
	}

    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * m->delta_time/2;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) *  m->delta_time/2;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) *  m->delta_time/2;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) *  m->delta_time/2;

    float norm2 = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 /= norm2;
    q1 /= norm2;
    q2 /= norm2;
    q3 /= norm2;

    m->q0 = q0;
    m->q1 = q1;
    m->q2 = q2;
    m->q3 = q3;
    m->exInt = exInt;
    m->eyInt = eyInt;
    m->ezInt = ezInt;
}

void Mahony_arm_math_cal(Mahony_TypeDef* m)
{
    float gx = m->gx, gy = m->gy, gz = m->gz;
    float ax = m->ax, ay = m->ay, az = m->az;
    float exInt = m->exInt, eyInt = m->eyInt, ezInt = m->ezInt;
    float q0 = m->q0, q1 = m->q1, q2 = m->q2, q3 = m->q3;

    // 四元数防 NaN
    if((q0 == 0.0f) && (q1 == 0.0f) && (q2 == 0.0f) && (q3 == 0.0f))
        q0 = 1.0f;

    // --- 加速度归一化 ---
    float acc_array[3] = {ax, ay, az};
    float acc_norm2;
    arm_dot_prod_f32(acc_array, acc_array, 3, &acc_norm2);
    float acc_norm;
    arm_sqrt_f32(acc_norm2, &acc_norm);

    float inv_acc = (acc_norm > 0.0001f) ? 1.0f / acc_norm : 1.0f;
    ax *= inv_acc;
    ay *= inv_acc;
    az *= inv_acc;

    // --- 方向余弦预估 ---
    float q0q1 = q0*q1, q0q2 = q0*q2, q0q3 = q0*q3;
    float q1q1 = q1*q1, q1q2 = q1*q2, q1q3 = q1*q3;
    float q2q2 = q2*q2, q2q3 = q2*q3, q3q3 = q3*q3;

    float vx = 2.0f*(q1q3 - q0q2);
    float vy = 2.0f*(q0q1 + q2q3);
    float vz = q0*q0 - q1q1 - q2q2 + q3q3;

    // --- 误差项 ---
    float ex = ay*vz - az*vy;
    float ey = az*vx - ax*vz;
    float ez = (ax*vy - ay*vx) + 0.05f*(ex + ey);

    // --- 积分项 ---
    exInt += ex * m->KI1;
    eyInt += ey * m->KI1;
    ezInt += ez * m->KI2;

    // 三元运算符替换 fminf/fmaxf
    exInt = (exInt > 0.05f) ? 0.05f : ((exInt < -0.05f) ? -0.05f : exInt);
    eyInt = (eyInt > 0.05f) ? 0.05f : ((eyInt < -0.05f) ? -0.05f : eyInt);
    ezInt = (ezInt > 0.05f) ? 0.05f : ((ezInt < -0.05f) ? -0.05f : ezInt);

    // --- PI+D 补偿 ---
    gx += m->KP1*ex + exInt;
    gy += m->KP1*ey + eyInt;
    gz += m->KP2*ez + ezInt;

    // --- 静止下零漂补偿 ---
    if(fabs(acc_norm - 1.0f) < 0.07f)
    {
        gz = 0.0f;
    }

    // --- 四元数更新 ---
    float halfT = 0.5f * m->delta_time;
    float dq0 = (-q1*gx - q2*gy - q3*gz) * halfT;
    float dq1 = ( q0*gx + q2*gz - q3*gy) * halfT;
    float dq2 = ( q0*gy - q1*gz + q3*gx) * halfT;
    float dq3 = ( q0*gz + q1*gy - q2*gx) * halfT;

    q0 += dq0;
    q1 += dq1;
    q2 += dq2;
    q3 += dq3;

    // --- 四元数归一化 ---
    float q_array[4] = {q0, q1, q2, q3};
    float q_norm2;
    arm_dot_prod_f32(q_array, q_array, 4, &q_norm2);
    float q_norm;
    arm_sqrt_f32(q_norm2, &q_norm);
    float inv_q = 1.0f / q_norm;

    q0 *= inv_q;
    q1 *= inv_q;
    q2 *= inv_q;
    q3 *= inv_q;

    // --- 写回 ---
    m->q0 = q0;
    m->q1 = q1;
    m->q2 = q2;
    m->q3 = q3;
    m->exInt = exInt;
    m->eyInt = eyInt;
    m->ezInt = ezInt;
}

void Mahony_to_euler(Mahony_TypeDef* m)
{
    float q0 = m->q0;
    float q1 = m->q1;
    float q2 = m->q2;
    float q3 = m->q3;

    // pitch
    float sinp = 2.0f * (q0 * q2 - q3 * q1);
    if(fabsf(sinp) >= 1.0f)
        m->pitch = copysignf(90.0f, sinp);
    else
        m->pitch = asinf(sinp) * 57.2957795f;

    // roll
    float roll_num = 2.0f * (q0*q1 + q2*q3);
    float roll_den = 1.0f - 2.0f * (q1*q1 + q2*q2);
    float roll_rad;
    arm_atan2_f32(roll_num, roll_den, &roll_rad);
    m->roll = roll_rad * 57.2957795f;

    // yaw
    float yaw_num = 2.0f * (q0*q3 + q1*q2);
    float yaw_den = 1.0f - 2.0f * (q2*q2 + q3*q3);
    float yaw_rad;
    arm_atan2_f32(yaw_num, yaw_den, &yaw_rad);
    m->yaw = yaw_rad * 57.2957795f;
}

void Mahony_exe()
{
	if(Mahony_Data.Mahony_Filiter[2].flag!=1)
	{
		Mahony_Error_Cal(&Mahony_Data.Mahony_Filiter[2],bmi088_data.gyro_rate.z);
	}
	else
	{
//		Mahony_adapt(&Mahony_Data,bmi088_data.gyro_rate.z);
		Mahony_update(&Mahony_Data,
		bmi088_data.gyro_rate.x,
		bmi088_data.gyro_rate.y,
		bmi088_data.gyro_rate.z-Mahony_Data.Mahony_Filiter[2].average,
		bmi088_data.acc.x,bmi088_data.acc.y,bmi088_data.acc.z);
		
		__HAL_TIM_SET_COUNTER(&htim1, 0);     
		__HAL_TIM_ENABLE(&htim1);
		
		Mahony_arm_math_cal(&Mahony_Data);
		Mahony_to_euler(&Mahony_Data);
		
		cal_time = __HAL_TIM_GET_COUNTER(&htim1);  // 读耗时
		__HAL_TIM_DISABLE(&htim1);            // 停止，不清 CNT
	}
}